
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_scurve.c
  * @author     baiyang
  * @date       2022-9-5
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_scurve.h"

#include <stdio.h>

#include <rtconfig.h>

#include <internal_error/internal_error.h>
/*-----------------------------------macro------------------------------------*/
#define SCURVE_SEG_INIT                0
#define SCURVE_SEG_ACCEL_MAX           4
#define SCURVE_SEG_TURN_IN             4
#define SCURVE_SEG_ACCEL_END           7
#define SCURVE_SEG_SPEED_CHANGE_END    14
#define SCURVE_SEG_CONST               15
#define SCURVE_SEG_TURN_OUT            15
#define SCURVE_SEG_DECEL_END           22
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void scurve_add_segment(scurve_t scurve, uint8_t *index, float end_time, enum SegmentType seg_type, float jerk_ref, float end_accel, float end_vel, float end_pos);
static void scurve_set_kinematic_limits(scurve_t scurve, const Vector3f_t *origin, const Vector3f_t *destination,
                                  float speed_xy, float speed_up, float speed_down,
                                  float accel_xy, float accel_z);
static bool scurve_valid(scurve_t scurve);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
// debugging messages
static void scurve_debug(scurve_t scurve);
#endif
static void scurve_add_segments(scurve_t scurve, float L);
static void scurve_add_segments_jerk(scurve_t scurve, uint8_t *index, float tj, float Jm, float Tcj);
static void scurve_add_segment_incr_jerk(scurve_t scurve, uint8_t *index, float tj, float Jm);
static void scurve_add_segment_const_jerk(scurve_t scurve, uint8_t *index, float tj, float J0);
static void scurve_add_segment_decr_jerk(scurve_t scurve, uint8_t *index, float tj, float Jm);
static void scurve_get_jerk_accel_vel_pos_at_time(scurve_t scurve, float time_now, float *Jt_out, float *At_out, float *Vt_out, float *Pt_out);
static void scurve_calc_javp_for_segment_const_jerk(float time_now, float J0, float A0, float V0, float P0, float *Jt, float *At, float *Vt, float *Pt);
static void scurve_calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float *Jt, float *At, float *Vt, float *Pt);
static void scurve_calc_javp_for_segment_decr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float *Jt, float *At, float *Vt, float *Pt);
static void scurve_move_from_pos_vel_accel(scurve_t scurve, float dt, Vector3f_t *pos, Vector3f_t *vel, Vector3f_t *accel);
static void scurve_move_to_pos_vel_accel(scurve_t scurve, float dt, Vector3f_t *pos, Vector3f_t *vel, Vector3f_t *accel);
static void scurve_move_from_time_pos_vel_accel(scurve_t scurve, float time_now, Vector3f_t *pos, Vector3f_t *vel, Vector3f_t *accel);
static inline float scurve_time_end(scurve_t scurve);
static inline float scurve_get_time_remaining(scurve_t scurve);
static inline float scurve_get_accel_finished_time(scurve_t scurve);
static inline bool scurve_braking(scurve_t scurve);
static inline float scurve_time_turn_in(scurve_t scurve);
static inline float scurve_time_turn_out(scurve_t scurve);
static inline void scurve_advance_time(scurve_t scurve, float dt);
static inline float scurve_get_speed_along_track(scurve_t scurve);
static inline float scurve_get_accel_along_track(scurve_t scurve);
static inline const Vector3f_t scurve_get_track(scurve_t scurve);
static inline float scurve_get_time_elapsed(scurve_t scurve);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/

void scurve_ctor(scurve_t scurve)
{
    scurve_init(scurve);
}

void scurve_init(scurve_t scurve)
{
    scurve->snap_max = 0.0f;
    scurve->jerk_max = 0.0f;
    scurve->accel_max = 0.0f;
    scurve->vel_max = 0.0f;
    scurve->time = 0.0f;
    scurve->num_segs = SCURVE_SEG_INIT;
    scurve_add_segment(scurve, &scurve->num_segs, 0.0f, SCURVE_CONSTANT_JERK, 0.0f, 0.0f, 0.0f, 0.0f);
    vec3_zero(&scurve->track);
    vec3_zero(&scurve->delta_unit);
    scurve->position_sq = 0.0f;
}

// generate a trigonometric track in 3D space that moves over a straight line
// between two points defined by the origin and destination
void scurve_calculate_track(scurve_t scurve, const Vector3f_t *origin, const Vector3f_t *destination,
                             float speed_xy, float speed_up, float speed_down,
                             float accel_xy, float accel_z,
                             float snap_maximum, float jerk_maximum)
{
    scurve_init(scurve);

    // leave track as zero length if origin and destination are equal or if the new track length squared is zero
    const Vector3f_t track_temp = {destination->x - origin->x,
                                   destination->y - origin->y,
                                   destination->z - origin->z};

    if (vec3_is_zero(&track_temp) || math_flt_zero(vec3_length_squared(&track_temp))) {
        return;
    }

    // set snap_max and jerk max
    scurve->snap_max = snap_maximum;
    scurve->jerk_max = jerk_maximum;

    // update speed and acceleration limits along path
    scurve_set_kinematic_limits(scurve, origin, destination,
                         speed_xy, speed_up, speed_down,
                         accel_xy, accel_z);

    // avoid divide-by zeros. Path will be left as a zero length path
    if (!math_flt_positive(scurve->snap_max) || !math_flt_positive(scurve->jerk_max) || !math_flt_positive(scurve->accel_max) || !math_flt_positive(scurve->vel_max)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SCurve::calculate_track created zero length path\n");
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        return;
    }

    scurve->track = track_temp;
    const float track_length = vec3_length(&scurve->track);
    if (math_flt_zero(track_length)) {
        // avoid possible divide by zero
        vec3_zero(&scurve->delta_unit);
    } else {
        vec3_norm(&scurve->delta_unit, &scurve->track);
        scurve_add_segments(scurve, track_length);
    }

    // catch calculation errors
    if (!scurve_valid(scurve)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SCurve::calculate_track invalid path\n");
        scurve_debug(scurve);
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        scurve_init(scurve);
    }
}

// calculate the segment times for the trigonometric S-Curve path defined by:
// Sm - duration of the raised cosine jerk profile
// Jm - maximum value of the raised cosine jerk profile
// V0 - initial velocity magnitude
// Am - maximum constant acceleration
// Vm - maximum constant velocity
// L - Length of the path
// tj_out, t2_out, t4_out, t6_out are the segment durations needed to achieve the kinematic path specified by the input variables
void scurve_calculate_path(float Sm, float Jm, float V0, float Am, float Vm, float L,float *Jm_out, float *tj_out,  float *t2_out, float *t4_out, float *t6_out)
{
    // init outputs
    *Jm_out = 0.0f;
    *tj_out = 0.0f;
    *t2_out = 0.0f;
    *t4_out = 0.0f;
    *t6_out = 0.0f;

    // check for invalid arguments
    if (!math_flt_positive(Sm) || !math_flt_positive(Jm) || !math_flt_positive(Am) || !math_flt_positive(Vm) || !math_flt_positive(L)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SCurve::calculate_path invalid inputs\n");
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        return;
    }

    if (V0 >= Vm) {
        // no velocity change so all segments as zero length
        return;
    }

    float tj = Jm * M_PI / (2 * Sm);
    float At = MIN(MIN(Am, 
        (Vm - V0) / (2.0f * tj) ), 
        (L + 4.0f * V0 * tj) / (4.0f * sq(tj)) );
    if (fabsf(At) < Jm * tj) {
        if (math_flt_zero(V0)) {
            // we do not have a solution for non-zero initial velocity
            tj = MIN( MIN( MIN( tj,
                powf((L * M_PI) / (8.0 * Sm), 1.0/4.0) ), 
                powf((Vm * M_PI) / (4.0 * Sm), 1.0/3.0) ), 
                math_sqrtf((Am * M_PI) / (2.0 * Sm)) );
            Jm = 2.0 * Sm * tj / M_PI;
            Am = Jm * tj;
        } else {
            // When doing speed change we use fixed tj and adjust Jm for small changes
            Am = At;
            Jm = Am / tj;
        }
        if ((Vm <= V0 + 2.0f * Am * tj) || (L <= 4.0f * V0 * tj + 4.0f * Am * sq(tj))) {
            // solution = 0 - t6 t4 t2 = 0 0 0
            *t2_out = 0.0f;
            *t4_out = 0.0f;
            *t6_out = 0.0f;
        } else {
            // solution = 2 - t6 t4 t2 = 0 1 0
            *t2_out = 0.0f;
            *t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + math_sqrtf((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - math_sqrtf((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm)));
            *t4_out = MAX(*t4_out, 0.0f);
            *t6_out = 0.0f;
        }
    } else {
        if ((Vm < V0 + Am * tj + (Am * Am) / Jm) || (L < 1.0f / (Jm * Jm) * (Am * Am * Am + Am * Jm * (V0 * 2.0f + Am * tj * 2.0f)) + V0 * tj * 2.0f + Am * (tj * tj))) {
            // solution = 5 - t6 t4 t2 = 1 0 1
            Am = MIN(MIN(Am, MAX(Jm * (tj + math_sqrtf((V0 * -4.0f + Vm * 4.0f + Jm * (tj * tj)) / Jm)) * (-1.0f / 2.0f), Jm * (tj - math_sqrtf((V0 * -4.0f + Vm * 4.0f + Jm * (tj * tj)) / Jm)) * (-1.0f / 2.0f))), Jm * tj * (-2.0f / 3.0f) + ((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f)) * 1.0f / powf(math_sqrtf(powf(- (Jm * Jm) * L * (1.0f / 2.0f) + (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) - Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) + (Jm * Jm) * V0 * tj, 2.0f) - powf((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f), 3.0f)) + (Jm * Jm) * L * (1.0f / 2.0f) - (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) + Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) - (Jm * Jm) * V0 * tj, 1.0f / 3.0f) + powf(math_sqrtf(powf(- (Jm * Jm) * L * (1.0f / 2.0f) + (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) - Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) + (Jm * Jm) * V0 * tj, 2.0f) - powf((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f), 3.0f)) + (Jm * Jm) * L * (1.0f / 2.0f) - (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) + Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) - (Jm * Jm) * V0 * tj, 1.0f / 3.0f));
            *t2_out = Am / Jm - tj;
            *t4_out = 0.0f;
            *t6_out = *t2_out;
        } else {
            // solution = 7 - t6 t4 t2 = 1 1 1
            *t2_out = Am / Jm - tj;
            *t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + math_sqrtf((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - math_sqrtf((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm)));
            *t4_out = MAX(*t4_out, 0.0f);
            *t6_out = *t2_out;
        }
    }
    *tj_out = tj;
    *Jm_out = Jm;

    // check outputs and reset back to zero if necessary
    if (!isfinite(*Jm_out) || math_flt_negative(*Jm_out) ||
        !isfinite(*tj_out) || math_flt_negative(*tj_out) ||
        !isfinite(*t2_out) || math_flt_negative(*t2_out) ||
        !isfinite(*t4_out) || math_flt_negative(*t4_out) ||
        !isfinite(*t6_out) || math_flt_negative(*t6_out)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SCurve::calculate_path invalid outputs\n");
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);

#if 0
        // @LoggerMessage: SCVE
        // @Description: Debug message for SCurve internal error
        // @Field: TimeUS: Time since system startup
        // @Field: Sm: duration of the raised cosine jerk profile
        // @Field: Jm: maximum value of the raised cosine jerk profile
        // @Field: V0: initial velocity magnitude
        // @Field: Am: maximum constant acceleration
        // @Field: Vm: maximum constant velocity
        // @Field: L: Length of the path
        // @Field: Jm_out: maximum value of the raised cosine jerk profile
        // @Field: tj_out: segment duration
        // @Field: t2_out: segment duration
        // @Field: t4_out: segment duration
        // @Field: t6_out: segment duration

        static bool logged_scve;  // only log once
        if (!logged_scve) {
            logged_scve = true;
            AP::logger().Write(
                "SCVE",
                "TimeUS,Sm,Jm,V0,Am,Vm,L,Jm_out,tj_out,t2_out,t4_out,t6_out",
                "s-----------",
                "F-----------",
                "Qfffffffffff",
                AP_HAL::micros64(),
                (double)Sm,
                (double)Jm,
                (double)V0,
                (double)Am,
                (double)Vm,
                (double)L,
                (double)*Jm_out,
                (double)*tj_out,
                (double)*t2_out,
                (double)*t4_out,
                (double)*t6_out
                );
        }
#endif  // APM_BUILD_COPTER_OR_HELI

        *Jm_out = 0.0f;
        *t2_out = 0.0f;
        *t4_out = 0.0f;
        *t6_out = 0.0f;
    }
}

// set maximum velocity and re-calculate the path using these limits
void scurve_set_speed_max(scurve_t scurve, float speed_xy, float speed_up, float speed_down)
{
    // return immediately if zero length path
    if (scurve->num_segs != segments_max) {
        return;
    }

    // segment accelerations can not be changed after segment creation.
    const float track_speed_max = control_kinematic_limit(scurve->delta_unit, speed_xy, speed_up, fabsf(speed_down));

    if (math_flt_equal(scurve->vel_max, track_speed_max)) {
        // new speed is equal to current speed maximum so no need to change anything
        return;
    }

    if (math_flt_zero(track_speed_max)) {
        // new speed is zero which is not supported
        return;
    }
    scurve->vel_max = track_speed_max;

    if (scurve->time >= scurve->segment[SCURVE_SEG_CONST].end_time) {
        return;
    }

    // re-calculate the s-curve path based on update speeds

    const float Pend = scurve->segment[SCURVE_SEG_DECEL_END].end_pos;
    float Vend = MIN(scurve->vel_max, scurve->segment[SCURVE_SEG_DECEL_END].end_vel);

    if (math_flt_zero(scurve->time)) {
        // path has not started so we can recompute the path
        const float Vstart = MIN(scurve->vel_max, scurve->segment[SCURVE_SEG_INIT].end_vel);
        scurve->num_segs = SCURVE_SEG_INIT;
        scurve_add_segment(scurve, &scurve->num_segs, 0.0f, SCURVE_CONSTANT_JERK, 0.0f, 0.0f, 0.0f, 0.0f);
        scurve_add_segments(scurve, Pend);
        scurve_set_origin_speed_max(scurve, Vstart);
        scurve_set_destination_speed_max(scurve, Vend);
        return;
    }

    if ((scurve->time >= scurve->segment[SCURVE_SEG_ACCEL_END].end_time) && (scurve->time <= scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_time)) {
        // in the speed change phase
        // move speed change phase to acceleration phase to provide room for further speed adjustments

        // set initial segment to last acceleration segment
        scurve->segment[SCURVE_SEG_INIT].seg_type = SCURVE_CONSTANT_JERK;
        scurve->segment[SCURVE_SEG_INIT].jerk_ref = 0.0f;
        scurve->segment[SCURVE_SEG_INIT].end_time = scurve->segment[SCURVE_SEG_ACCEL_END].end_time;
        scurve->segment[SCURVE_SEG_INIT].end_accel = scurve->segment[SCURVE_SEG_ACCEL_END].end_accel;
        scurve->segment[SCURVE_SEG_INIT].end_vel = scurve->segment[SCURVE_SEG_ACCEL_END].end_vel;
        scurve->segment[SCURVE_SEG_INIT].end_pos = scurve->segment[SCURVE_SEG_ACCEL_END].end_pos;

        // move speed change segments to acceleration segments
        for (uint8_t i = SCURVE_SEG_INIT+1; i <= SCURVE_SEG_ACCEL_END; i++) {
            scurve->segment[i] = scurve->segment[i+7];
        }

        // set change segments to last acceleration speed
        for (uint8_t i = SCURVE_SEG_ACCEL_END+1; i <= SCURVE_SEG_SPEED_CHANGE_END; i++) {
            scurve->segment[i].seg_type = SCURVE_CONSTANT_JERK;
            scurve->segment[i].jerk_ref = 0.0f;
            scurve->segment[i].end_time = scurve->segment[SCURVE_SEG_ACCEL_END].end_time;
            scurve->segment[i].end_accel = 0.0f;
            scurve->segment[i].end_vel = scurve->segment[SCURVE_SEG_ACCEL_END].end_vel;
            scurve->segment[i].end_pos = scurve->segment[SCURVE_SEG_ACCEL_END].end_pos;
        }

    } else if ((scurve->time > scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_time) && (scurve->time <= scurve->segment[SCURVE_SEG_CONST].end_time)) {
        // in the constant speed phase
        // overwrite the acceleration and speed change phases with the current position and velocity

        // set initial segment to last acceleration segment
        scurve->segment[SCURVE_SEG_INIT].seg_type = SCURVE_CONSTANT_JERK;
        scurve->segment[SCURVE_SEG_INIT].jerk_ref = 0.0f;
        scurve->segment[SCURVE_SEG_INIT].end_time = scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_time;
        scurve->segment[SCURVE_SEG_INIT].end_accel = 0.0f;
        scurve->segment[SCURVE_SEG_INIT].end_vel = scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_vel;
        scurve->segment[SCURVE_SEG_INIT].end_pos = scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_pos;

        // set acceleration and change segments to current constant speed
        float Jt_out, At_out, Vt_out, Pt_out;
        scurve_get_jerk_accel_vel_pos_at_time(scurve, scurve->time, &Jt_out, &At_out, &Vt_out, &Pt_out);
        for (uint8_t i = SCURVE_SEG_INIT+1; i <= SCURVE_SEG_SPEED_CHANGE_END; i++) {
            scurve->segment[i].seg_type = SCURVE_CONSTANT_JERK;
            scurve->segment[i].jerk_ref = 0.0f;
            scurve->segment[i].end_time = scurve->time;
            scurve->segment[i].end_accel = 0.0f;
            scurve->segment[i].end_vel = Vt_out;
            scurve->segment[i].end_pos = Pt_out;
        }
    }

    // adjust the INIT and ACCEL segments for new speed
    if ((scurve->time <= scurve->segment[SCURVE_SEG_ACCEL_MAX].end_time) && math_flt_positive(scurve->segment[SCURVE_SEG_ACCEL_MAX].end_time - scurve->segment[SCURVE_SEG_ACCEL_MAX-1].end_time) && (scurve->vel_max < scurve->segment[SCURVE_SEG_ACCEL_END].end_vel) && math_flt_positive(scurve->segment[SCURVE_SEG_ACCEL_MAX].end_accel) ) {
        // path has not finished constant positive acceleration segment
        // reduce velocity as close to target velocity as possible

        const float Vstart = scurve->segment[SCURVE_SEG_INIT].end_vel;

        // minimum velocity that can be obtained by shortening SEG_ACCEL_MAX
        const float Vmin = scurve->segment[SCURVE_SEG_ACCEL_END].end_vel - scurve->segment[SCURVE_SEG_ACCEL_MAX].end_accel * (scurve->segment[SCURVE_SEG_ACCEL_MAX].end_time - MAX(scurve->time, scurve->segment[SCURVE_SEG_ACCEL_MAX-1].end_time));

        float Jm, tj, t2, t4, t6;
        scurve_calculate_path(scurve->snap_max, scurve->jerk_max, Vstart, scurve->accel_max, MAX(Vmin, scurve->vel_max), Pend * 0.5f, &Jm, &tj, &t2, &t4, &t6);

        uint8_t seg = SCURVE_SEG_INIT+1;
        scurve_add_segments_jerk(scurve, &seg, tj, Jm, t2);
        scurve_add_segment_const_jerk(scurve, &seg, t4, 0.0f);
        scurve_add_segments_jerk(scurve, &seg, tj, -Jm, t6);

        // remove numerical errors
        scurve->segment[SCURVE_SEG_ACCEL_END].end_accel = 0.0f;

        // add empty speed adjust segments
        for (uint8_t i = SCURVE_SEG_ACCEL_END+1; i <= SCURVE_SEG_CONST; i++) {
            scurve->segment[i].seg_type = SCURVE_CONSTANT_JERK;
            scurve->segment[i].jerk_ref = 0.0f;
            scurve->segment[i].end_time = scurve->segment[SCURVE_SEG_ACCEL_END].end_time;
            scurve->segment[i].end_accel = 0.0f;
            scurve->segment[i].end_vel = scurve->segment[SCURVE_SEG_ACCEL_END].end_vel;
            scurve->segment[i].end_pos = scurve->segment[SCURVE_SEG_ACCEL_END].end_pos;
        }

        scurve_calculate_path(scurve->snap_max, scurve->jerk_max, 0.0f, scurve->accel_max, MAX(Vmin, scurve->vel_max), Pend * 0.5f, &Jm, &tj, &t2, &t4, &t6);

        seg = SCURVE_SEG_CONST + 1;
        scurve_add_segments_jerk(scurve, &seg, tj, -Jm, t6);
        scurve_add_segment_const_jerk(scurve, &seg, t4, 0.0f);
        scurve_add_segments_jerk(scurve, &seg, tj, Jm, t2);

        // remove numerical errors
        scurve->segment[SCURVE_SEG_DECEL_END].end_accel = 0.0f;
        scurve->segment[SCURVE_SEG_DECEL_END].end_vel = MAX(0.0f, scurve->segment[SCURVE_SEG_DECEL_END].end_vel);

        // add to constant velocity segment to end at the correct position
        const float dP = MAX(0.0f, Pend - scurve->segment[SCURVE_SEG_DECEL_END].end_pos);
        const float t15 =  dP / scurve->segment[SCURVE_SEG_CONST].end_vel;
        for (uint8_t i = SCURVE_SEG_CONST; i <= SCURVE_SEG_DECEL_END; i++) {
            scurve->segment[i].end_time += t15;
            scurve->segment[i].end_pos += dP;
        }
    }

    // adjust the speed change segments (8 to 14) for new speed
    // start with empty speed adjust segments
    for (uint8_t i = SCURVE_SEG_ACCEL_END+1; i <= SCURVE_SEG_SPEED_CHANGE_END; i++) {
        scurve->segment[i].seg_type = SCURVE_CONSTANT_JERK;
        scurve->segment[i].jerk_ref = 0.0f;
        scurve->segment[i].end_time = scurve->segment[SCURVE_SEG_ACCEL_END].end_time;
        scurve->segment[i].end_accel = 0.0f;
        scurve->segment[i].end_vel = scurve->segment[SCURVE_SEG_ACCEL_END].end_vel;
        scurve->segment[i].end_pos = scurve->segment[SCURVE_SEG_ACCEL_END].end_pos;
    }
    if (!math_flt_equal(scurve->vel_max, scurve->segment[SCURVE_SEG_ACCEL_END].end_vel)) {
        // add velocity adjustment
        // check there is enough time to make velocity change
        // we use the approximation that the time will be distance/max_vel and 8 jerk segments
        const float L = scurve->segment[SCURVE_SEG_CONST].end_pos - scurve->segment[SCURVE_SEG_ACCEL_END].end_pos;
        float Jm = 0;
        float tj = 0;
        float t2 = 0;
        float t4 = 0;
        float t6 = 0;
        float jerk_time = MIN(powf((fabsf(scurve->vel_max - scurve->segment[SCURVE_SEG_ACCEL_END].end_vel) * M_PI) / (4 * scurve->snap_max), 1/3), scurve->jerk_max * M_PI / (2 * scurve->snap_max));
        if ((scurve->vel_max < scurve->segment[SCURVE_SEG_ACCEL_END].end_vel) && (jerk_time*12.0f < L/scurve->segment[SCURVE_SEG_ACCEL_END].end_vel)) {
            // we have a problem here with small segments.
            scurve_calculate_path(scurve->snap_max, scurve->jerk_max, scurve->vel_max, scurve->accel_max, scurve->segment[SCURVE_SEG_ACCEL_END].end_vel, L * 0.5f, &Jm, &tj, &t6, &t4, &t2);
            Jm = -Jm;

        } else if ((scurve->vel_max > scurve->segment[SCURVE_SEG_ACCEL_END].end_vel) && (L/(jerk_time*12.0f) > scurve->segment[SCURVE_SEG_ACCEL_END].end_vel)) {
            float Vm = MIN(scurve->vel_max, L/(jerk_time*12.0f));
            scurve_calculate_path(scurve->snap_max, scurve->jerk_max, scurve->segment[SCURVE_SEG_ACCEL_END].end_vel, scurve->accel_max, Vm, L * 0.5f, &Jm, &tj, &t2, &t4, &t6);
        }

        uint8_t seg = SCURVE_SEG_ACCEL_END + 1;
        if (!math_flt_zero(Jm) && !math_flt_negative(t2) && !math_flt_negative(t4) && !math_flt_negative(t6)) {
            scurve_add_segments_jerk(scurve, &seg, tj, Jm, t2);
            scurve_add_segment_const_jerk(scurve, &seg, t4, 0.0f);
            scurve_add_segments_jerk(scurve, &seg, tj, -Jm, t6);

            // remove numerical errors
            scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_accel = 0.0f;
        }
    }

    // add deceleration segments
    // earlier check should ensure that we should always have sufficient time to stop
    uint8_t seg = SCURVE_SEG_CONST;
    Vend = MIN(Vend, scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_vel);
    scurve_add_segment_const_jerk(scurve, &seg, 0.0f, 0.0f);
    if (Vend < scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_vel) {
        float Jm, tj, t2, t4, t6;
        scurve_calculate_path(scurve->snap_max, scurve->jerk_max, Vend, scurve->accel_max, scurve->segment[SCURVE_SEG_CONST].end_vel, Pend - scurve->segment[SCURVE_SEG_CONST].end_pos, &Jm, &tj, &t2, &t4, &t6);
        scurve_add_segments_jerk(scurve, &seg, tj, -Jm, t6);
        scurve_add_segment_const_jerk(scurve, &seg, t4, 0.0f);
        scurve_add_segments_jerk(scurve, &seg, tj, Jm, t2);
    } else {
        // No deceleration is required
        for (uint8_t i = SCURVE_SEG_CONST+1; i <= SCURVE_SEG_DECEL_END; i++) {
            scurve->segment[i].seg_type = SCURVE_CONSTANT_JERK;
            scurve->segment[i].jerk_ref = 0.0f;
            scurve->segment[i].end_time = scurve->segment[SCURVE_SEG_CONST].end_time;
            scurve->segment[i].end_accel = 0.0f;
            scurve->segment[i].end_vel = scurve->segment[SCURVE_SEG_CONST].end_vel;
            scurve->segment[i].end_pos = scurve->segment[SCURVE_SEG_CONST].end_pos;
        }
    }

    // remove numerical errors
    scurve->segment[SCURVE_SEG_DECEL_END].end_accel = 0.0f;
    scurve->segment[SCURVE_SEG_DECEL_END].end_vel = MAX(0.0f, scurve->segment[SCURVE_SEG_DECEL_END].end_vel);

    // add to constant velocity segment to end at the correct position
    const float dP = MAX(0.0f, Pend - scurve->segment[SCURVE_SEG_DECEL_END].end_pos);
    const float t15 =  dP / scurve->segment[SCURVE_SEG_CONST].end_vel;
    for (uint8_t i = SCURVE_SEG_CONST; i <= SCURVE_SEG_DECEL_END; i++) {
        scurve->segment[i].end_time += t15;
        scurve->segment[i].end_pos += dP;
    }

    // catch calculation errors
    if (!scurve_valid(scurve)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SCurve::set_speed_max invalid path\n");
        scurve_debug(scurve);
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        scurve_init(scurve);
    }
}

// set the maximum vehicle speed at the origin
// returns the expected speed at the origin which will always be equal or lower than speed
float scurve_set_origin_speed_max(scurve_t scurve, float speed)
{
    // if path is zero length then start speed must be zero
    if (scurve->num_segs != segments_max) {
        return 0.0f;
    }

    // avoid re-calculating if unnecessary
    if (math_flt_equal(scurve->segment[SCURVE_SEG_INIT].end_vel, speed)) {
        return speed;
    }

    const float Vm = scurve->segment[SCURVE_SEG_ACCEL_END].end_vel;
    const float track_length = vec3_length(&scurve->track);
    speed = MIN(speed, Vm);

    float Jm, tj, t2, t4, t6;
    scurve_calculate_path(scurve->snap_max, scurve->jerk_max, speed, scurve->accel_max, Vm, track_length * 0.5f, &Jm, &tj, &t2, &t4, &t6);

    uint8_t seg = SCURVE_SEG_INIT;
    scurve_add_segment(scurve, &seg, 0.0f, SCURVE_CONSTANT_JERK, 0.0f, 0.0f, speed, 0.0f);
    scurve_add_segments_jerk(scurve, &seg, tj, Jm, t2);
    scurve_add_segment_const_jerk(scurve, &seg, t4, 0.0f);
    scurve_add_segments_jerk(scurve, &seg, tj, -Jm, t6);

    // remove numerical errors
    scurve->segment[SCURVE_SEG_ACCEL_END].end_accel = 0.0f;

    // offset acceleration segment if we can't fit it all into half the original length
    const float dPstart = MIN(0.0f, track_length * 0.5f - scurve->segment[SCURVE_SEG_ACCEL_END].end_pos);
    const float dt =  dPstart / scurve->segment[SCURVE_SEG_ACCEL_END].end_vel;
    for (uint8_t i = SCURVE_SEG_INIT; i <= SCURVE_SEG_ACCEL_END; i++) {
        scurve->segment[i].end_time += dt;
        scurve->segment[i].end_pos += dPstart;
    }

    // add empty speed change segments and constant speed segment
    for (uint8_t i = SCURVE_SEG_ACCEL_END+1; i <= SCURVE_SEG_SPEED_CHANGE_END; i++) {
        scurve->segment[i].seg_type = SCURVE_CONSTANT_JERK;
        scurve->segment[i].jerk_ref = 0.0f;
        scurve->segment[i].end_time = scurve->segment[SCURVE_SEG_ACCEL_END].end_time;
        scurve->segment[i].end_accel = 0.0f;
        scurve->segment[i].end_vel = scurve->segment[SCURVE_SEG_ACCEL_END].end_vel;
        scurve->segment[i].end_pos = scurve->segment[SCURVE_SEG_ACCEL_END].end_pos;
    }

    seg = SCURVE_SEG_CONST;
    scurve_add_segment_const_jerk(scurve, &seg, 0.0f, 0.0f);

    scurve_calculate_path(scurve->snap_max, scurve->jerk_max, 0.0f, scurve->accel_max, scurve->segment[SCURVE_SEG_CONST].end_vel, track_length * 0.5f, &Jm, &tj, &t2, &t4, &t6);

    scurve_add_segments_jerk(scurve, &seg, tj, -Jm, t6);
    scurve_add_segment_const_jerk(scurve, &seg, t4, 0.0f);
    scurve_add_segments_jerk(scurve, &seg, tj, Jm, t2);

    // remove numerical errors
    scurve->segment[SCURVE_SEG_DECEL_END].end_accel = 0.0f;
    scurve->segment[SCURVE_SEG_DECEL_END].end_vel = MAX(0.0f, scurve->segment[SCURVE_SEG_DECEL_END].end_vel);

    // add to constant velocity segment to end at the correct position
    const float dP = MAX(0.0f, track_length - scurve->segment[SCURVE_SEG_DECEL_END].end_pos);
    const float t15 =  dP / scurve->segment[SCURVE_SEG_CONST].end_vel;
    for (uint8_t i = SCURVE_SEG_CONST; i <= SCURVE_SEG_DECEL_END; i++) {
        scurve->segment[i].end_time += t15;
        scurve->segment[i].end_pos += dP;
    }

    // catch calculation errors
    if (!scurve_valid(scurve)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SCurve::set_origin_speed_max invalid path\n");
        scurve_debug(scurve);
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        scurve_init(scurve);
        return 0.0f;
    }

    return speed;
}

// set the maximum vehicle speed at the destination
void scurve_set_destination_speed_max(scurve_t scurve, float speed)
{
    // if path is zero length then all speeds must be zero
    if (scurve->num_segs != segments_max) {
        return;
    }

    // avoid re-calculating if unnecessary
    if (math_flt_equal(scurve->segment[segments_max-1].end_vel, speed)) {
        return;
    }

    const float Vm = scurve->segment[SCURVE_SEG_CONST].end_vel;
    const float track_length = vec3_length(&scurve->track);
    speed = MIN(speed, Vm);

    float Jm, tj, t2, t4, t6;
    scurve_calculate_path(scurve->snap_max, scurve->jerk_max, speed, scurve->accel_max, Vm, track_length * 0.5f, &Jm, &tj, &t2, &t4, &t6);

    uint8_t seg = SCURVE_SEG_CONST;
    scurve_add_segment_const_jerk(scurve, &seg, 0.0f, 0.0f);

    scurve_add_segments_jerk(scurve, &seg, tj, -Jm, t6);
    scurve_add_segment_const_jerk(scurve, &seg, t4, 0.0f);
    scurve_add_segments_jerk(scurve, &seg, tj, Jm, t2);

    // remove numerical errors
    scurve->segment[SCURVE_SEG_DECEL_END].end_accel = 0.0f;
    scurve->segment[SCURVE_SEG_DECEL_END].end_vel = MAX(0.0f, scurve->segment[SCURVE_SEG_DECEL_END].end_vel);

    // add to constant velocity segment to end at the correct position
    const float dP = MAX(0.0f, track_length - scurve->segment[SCURVE_SEG_DECEL_END].end_pos);
    const float t15 =  dP / scurve->segment[SCURVE_SEG_CONST].end_vel;
    for (uint8_t i = SCURVE_SEG_CONST; i <= SCURVE_SEG_DECEL_END; i++) {
        scurve->segment[i].end_time += t15;
        scurve->segment[i].end_pos += dP;
    }

    // catch calculation errors
    if (!scurve_valid(scurve)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SCurve::set_destination_speed_max invalid path\n");
        scurve_debug(scurve);
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        scurve_init(scurve);
    }
}

// move target location along path from origin to destination
// prev_leg and next_leg are the paths before and after this path
// wp_radius is max distance from the waypoint at the apex of the turn
// fast_waypoint should be true if vehicle will not stop at end of this leg
// dt is the time increment the vehicle will move along the path
// target_pos should be set to this segment's origin and it will be updated to the current position target
// target_vel and target_accel are updated with new targets
// returns true if vehicle has passed the apex of the corner
bool scurve_advance_target_along_track(scurve_t scurve, scurve_t prev_leg, scurve_t next_leg, float wp_radius, float accel_corner, bool fast_waypoint, float dt, Vector3f_t *target_pos, Vector3f_t *target_vel, Vector3f_t *target_accel)
{
    scurve_move_to_pos_vel_accel(prev_leg, dt, target_pos, target_vel, target_accel);
    scurve_move_from_pos_vel_accel(scurve, dt, target_pos, target_vel, target_accel);
    bool s_finished = scurve_finished(scurve);

    // check for change of leg on fast waypoint
    const float time_to_destination = scurve_get_time_remaining(scurve);
    if (fast_waypoint 
        && math_flt_zero(scurve_get_time_elapsed(next_leg)) 
        && (scurve_get_time_elapsed(scurve) >= scurve_time_turn_out(scurve) - scurve_time_turn_in(next_leg)) 
        && (scurve->position_sq >= 0.25 * vec3_length_squared(&scurve->track))) {

        Vector3f_t turn_pos = {-scurve->track.x, -scurve->track.y, -scurve->track.z};
        Vector3f_t turn_vel = VECTOR3F_DEFAULT_VALUE, turn_accel = VECTOR3F_DEFAULT_VALUE;
        scurve_move_from_time_pos_vel_accel(scurve, scurve_get_time_elapsed(scurve) + time_to_destination * 0.5f, &turn_pos, &turn_vel, &turn_accel);
        scurve_move_from_time_pos_vel_accel(next_leg, time_to_destination * 0.5f, &turn_pos, &turn_vel, &turn_accel);
        const float speed_min = MIN(scurve_get_speed_along_track(scurve), scurve_get_speed_along_track(next_leg));
        if ((scurve_get_time_remaining(scurve) < scurve_time_end(next_leg) * 0.5f) && (vec3_length(&turn_pos) < wp_radius) &&
             (sqrtf(turn_vel.x * turn_vel.x + turn_vel.y * turn_vel.y) < speed_min) &&
             (sqrtf(turn_accel.x * turn_accel.x + turn_accel.y * turn_accel.y) < accel_corner)) {
            scurve_move_from_pos_vel_accel(next_leg, dt, target_pos, target_vel, target_accel);
        }
    } else if (!math_flt_zero(scurve_get_time_elapsed(next_leg))) {
        scurve_move_from_pos_vel_accel(next_leg, dt, target_pos, target_vel, target_accel);
        if (scurve_get_time_elapsed(next_leg) >= scurve_get_time_remaining(scurve)) {
            s_finished = true;
        }
    }

    return s_finished;
}

// time has reached the end of the sequence
bool scurve_finished(scurve_t scurve)
{
    return ((scurve->time >= scurve_time_end(scurve)) || (scurve->position_sq >= vec3_length_squared(&scurve->track)));
}

// add single S-Curve segment
// populate the information for the segment specified in the path by the index variable.
// the index variable is incremented to reference the next segment in the array
static void scurve_add_segment(scurve_t scurve, uint8_t *index, float end_time, enum SegmentType seg_type, float jerk_ref, float end_accel, float end_vel, float end_pos)
{
    scurve->segment[*index].end_time = end_time;
    scurve->segment[*index].seg_type = seg_type;
    scurve->segment[*index].jerk_ref = jerk_ref;
    scurve->segment[*index].end_accel = end_accel;
    scurve->segment[*index].end_vel = end_vel;
    scurve->segment[*index].end_pos = end_pos;
    *index = *index + 1;
}

// set speed and acceleration limits for the path
// origin and destination are offsets from EKF origin
// speed and acceleration parameters are given in horizontal, up and down.
static void scurve_set_kinematic_limits(scurve_t scurve, const Vector3f_t *origin, const Vector3f_t *destination,
                                  float speed_xy, float speed_up, float speed_down,
                                  float accel_xy, float accel_z)
{
    // ensure arguments are positive
    speed_xy = fabsf(speed_xy);
    speed_up = fabsf(speed_up);
    speed_down = fabsf(speed_down);
    accel_xy = fabsf(accel_xy);
    accel_z = fabsf(accel_z);

    Vector3f_t direction = {destination->x - origin->x,
                            destination->y - origin->y,
                            destination->z - origin->z};

    const float track_speed_max = control_kinematic_limit(direction, speed_xy, speed_up, speed_down);
    const float track_accel_max = control_kinematic_limit(direction, accel_xy, accel_z, accel_z);

    scurve->vel_max = track_speed_max;
    scurve->accel_max = track_accel_max;
}

// return true if the curve is valid.  Used to identify and protect against code errors
static bool scurve_valid(scurve_t scurve)
{
    // check number of segments
    if (scurve->num_segs != segments_max) {
        return false;
    }

    for (uint8_t i = 0; i < scurve->num_segs; i++) {
        // jerk_ref should be finite (i.e. not NaN or infinity)
        // time, accel, vel and pos should finite and not negative
        if (!isfinite(scurve->segment[i].jerk_ref) ||
            !isfinite(scurve->segment[i].end_time) ||
            !isfinite(scurve->segment[i].end_accel) ||
            !isfinite(scurve->segment[i].end_vel) || math_flt_negative(scurve->segment[i].end_vel) ||
            !isfinite(scurve->segment[i].end_pos)) {
            return false;
        }

        // time and pos should be increasing
        if (i >= 1) {
            if (math_flt_negative(scurve->segment[i].end_time - scurve->segment[i-1].end_time) ||
                math_flt_negative(scurve->segment[i].end_pos - scurve->segment[i-1].end_pos)) {
                return false;
            }
        }
    }

    // last segment should have zero acceleration
    if (!math_flt_zero(scurve->segment[scurve->num_segs-1].end_accel)) {
        return false;
    }

    // if we get this far then the curve must be valid
    return true;
}

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
// debugging messages
static void scurve_debug(scurve_t scurve)
{
    printf("num_segs:%u, time:%4.2f, snap_max:%4.2f, jerk_max:%4.2f, accel_max:%4.2f, vel_max:%4.2f\n",
                        (unsigned)scurve->num_segs, (double)scurve->time, (double)scurve->snap_max, (double)scurve->jerk_max, (double)scurve->accel_max, (double)scurve->vel_max);
    printf("T, Jt, J, A, V, P \n");
    for (uint8_t i = 0; i < scurve->num_segs; i++) {
        printf("i:%u, T:%4.2f, Jtype:%4.2f, J:%4.2f, A:%4.2f, V: %4.2f, P: %4.2f\n",
                            (unsigned)i, (double)scurve->segment[i].end_time, (double)scurve->segment[i].seg_type, (double)scurve->segment[i].jerk_ref,
                            (double)scurve->segment[i].end_accel, (double)scurve->segment[i].end_vel, (double)scurve->segment[i].end_pos);
    }
    printf("track x:%4.2f, y:%4.2f, z:%4.2f\n", (double)scurve->track.x, (double)scurve->track.y, (double)scurve->track.z);
    printf("delta_unit x:%4.2f, y:%4.2f, z:%4.2f\n", (double)scurve->delta_unit.x, (double)scurve->delta_unit.y, (double)scurve->delta_unit.z);
}
#endif

// generate the segments for a path of length L
// the path consists of 23 segments
// 1 initial segment
// 7 segments forming the acceleration S-Curve
// 7 segments forming the velocity change S-Curve
// 1 constant velocity S-Curve
// 7 segments forming the deceleration S-Curve
static void scurve_add_segments(scurve_t scurve, float L)
{
    if (math_flt_zero(L)) {
        return;
    }

    float Jm, tj, t2, t4, t6;
    scurve_calculate_path(scurve->snap_max, scurve->jerk_max, 0.0f, scurve->accel_max, scurve->vel_max, L * 0.5f, &Jm, &tj, &t2, &t4, &t6);

    scurve_add_segments_jerk(scurve, &scurve->num_segs, tj, Jm, t2);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, t4, 0.0f);
    scurve_add_segments_jerk(scurve, &scurve->num_segs, tj, -Jm, t6);

    // remove numerical errors
    scurve->segment[SCURVE_SEG_ACCEL_END].end_accel = 0.0f;

    // add empty speed adjust segments
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, 0.0f, 0.0f);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, 0.0f, 0.0f);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, 0.0f, 0.0f);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, 0.0f, 0.0f);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, 0.0f, 0.0f);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, 0.0f, 0.0f);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, 0.0f, 0.0f);

    const float t15 = MAX(0.0f, (L - 2.0f * scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_pos) / scurve->segment[SCURVE_SEG_SPEED_CHANGE_END].end_vel);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, t15, 0.0f);

    scurve_add_segments_jerk(scurve, &scurve->num_segs, tj, -Jm, t6);
    scurve_add_segment_const_jerk(scurve, &scurve->num_segs, t4, 0.0f);
    scurve_add_segments_jerk(scurve, &scurve->num_segs, tj, Jm, t2);

    // remove numerical errors
    scurve->segment[SCURVE_SEG_DECEL_END].end_accel = 0.0f;
    scurve->segment[SCURVE_SEG_DECEL_END].end_vel = 0.0f;
}

// generate three consecutive segments forming a jerk profile
// the index variable is the position within the path array that this jerk profile should be added
// the index is incremented to reference the next segment in the array after the jerk profile
static void scurve_add_segments_jerk(scurve_t scurve, uint8_t *index, float tj, float Jm, float Tcj)
{
    scurve_add_segment_incr_jerk(scurve, index, tj, Jm);
    scurve_add_segment_const_jerk(scurve, index, Tcj, Jm);
    scurve_add_segment_decr_jerk(scurve, index, tj, Jm);
}

// generate increasing jerk magnitude time segment based on a raised cosine profile
// calculate the information needed to populate the increasing jerk magnitude segment from the segment duration tj and jerk magnitude Jm
// the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array
static void scurve_add_segment_incr_jerk(scurve_t scurve, uint8_t *index, float tj, float Jm)
{
    // if no time increase copy previous segment
    if (!math_flt_positive(tj)) {
        scurve_add_segment(scurve, index, scurve->segment[*index - 1].end_time,
                    SCURVE_CONSTANT_JERK,
                    0.0,
                    scurve->segment[*index - 1].end_accel,
                    scurve->segment[*index - 1].end_vel,
                    scurve->segment[*index - 1].end_pos);
        return;
    }
    const float Beta = M_PI / tj;
    const float Alpha = Jm * 0.5f;
    const float AT = Alpha * tj;
    const float VT = Alpha * (sq(tj) * 0.5f - 2.0f / sq(Beta));
    const float PT = Alpha * ((-1.0f / sq(Beta)) * tj + (1.0f / 6.0f) * powf(tj, 3.0f));

    const float J = Jm;
    const float T = scurve->segment[*index - 1].end_time + tj;
    const float A = scurve->segment[*index - 1].end_accel + AT;
    const float V = scurve->segment[*index - 1].end_vel + scurve->segment[*index - 1].end_accel * tj + VT;
    const float P = scurve->segment[*index - 1].end_pos + scurve->segment[*index - 1].end_vel * tj + 0.5f * scurve->segment[*index - 1].end_accel * sq(tj) + PT;
    scurve_add_segment(scurve, index, T, SCURVE_POSITIVE_JERK, J, A, V, P);
}

// generate constant jerk time segment
// calculate the information needed to populate the constant jerk segment from the segment duration tj and jerk J0
// the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array
static void scurve_add_segment_const_jerk(scurve_t scurve, uint8_t *index, float tj, float J0)
{
    // if no time increase copy previous segment
    if (!math_flt_positive(tj)) {
        scurve_add_segment(scurve, index, scurve->segment[*index - 1].end_time,
                    SCURVE_CONSTANT_JERK,
                    J0,
                    scurve->segment[*index - 1].end_accel,
                    scurve->segment[*index - 1].end_vel,
                    scurve->segment[*index - 1].end_pos);
        return;
    }

    const float J = J0;
    const float T = scurve->segment[*index - 1].end_time + tj;
    const float A = scurve->segment[*index - 1].end_accel + J0 * tj;
    const float V = scurve->segment[*index - 1].end_vel + scurve->segment[*index - 1].end_accel * tj + 0.5f * J0 * sq(tj);
    const float P = scurve->segment[*index - 1].end_pos + scurve->segment[*index - 1].end_vel * tj + 0.5f * scurve->segment[*index - 1].end_accel * sq(tj) + (1.0f / 6.0f) * J0 * powf(tj, 3.0f);
    scurve_add_segment(scurve, index, T, SCURVE_CONSTANT_JERK, J, A, V, P);
}

// generate decreasing jerk magnitude time segment based on a raised cosine profile
// calculate the information needed to populate the decreasing jerk magnitude segment from the segment duration tj and jerk magnitude Jm
// the index variable is the position of this segment in the path and is incremented to reference the next segment in the array
static void scurve_add_segment_decr_jerk(scurve_t scurve, uint8_t *index, float tj, float Jm)
{
    // if no time increase copy previous segment
    if (!math_flt_positive(tj)) {
        scurve_add_segment(scurve, index, scurve->segment[*index - 1].end_time,
                    SCURVE_CONSTANT_JERK,
                    0.0,
                    scurve->segment[*index - 1].end_accel,
                    scurve->segment[*index - 1].end_vel,
                    scurve->segment[*index - 1].end_pos);
        return;
    }
    const float Beta = M_PI / tj;
    const float Alpha = Jm * 0.5f;
    const float AT = Alpha * tj;
    const float VT = Alpha * (sq(tj) * 0.5f - 2.0f / sq(Beta));
    const float PT = Alpha * ((-1.0f / sq(Beta)) * tj + (1.0f / 6.0f) * powf(tj, 3.0f));
    const float A2T = Jm * tj;
    const float V2T = Jm * sq(tj);
    const float P2T = Alpha * ((-1.0f / sq(Beta)) * 2.0f * tj + (4.0f / 3.0f) * powf(tj, 3.0f));

    const float J = Jm;
    const float T = scurve->segment[*index - 1].end_time + tj;
    const float A = (scurve->segment[*index - 1].end_accel - AT) + A2T;
    const float V = (scurve->segment[*index - 1].end_vel - VT) + (scurve->segment[*index - 1].end_accel - AT) * tj + V2T;
    const float P = (scurve->segment[*index - 1].end_pos - PT) + (scurve->segment[*index - 1].end_vel - VT) * tj + 0.5f * (scurve->segment[*index - 1].end_accel - AT) * sq(tj) + P2T;
    scurve_add_segment(scurve, index, T, SCURVE_NEGATIVE_JERK, J, A, V, P);
}

// calculate the jerk, acceleration, velocity and position at the provided time
static void scurve_get_jerk_accel_vel_pos_at_time(scurve_t scurve, float time_now, float *Jt_out, float *At_out, float *Vt_out, float *Pt_out)
{
    // start with zeros as function is void and we want to guarantee all outputs are initialised
    *Jt_out = 0;
    *At_out = 0;
    *Vt_out = 0;
    *Pt_out = 0;
    if (scurve->num_segs != segments_max) {
        return;
    }

    enum SegmentType Jtype;
    uint8_t pnt = scurve->num_segs;
    float Jm, tj, T0, A0, V0, P0;

    // find active segment at time_now
    for (uint8_t i = 0; i < scurve->num_segs; i++) {
        if (time_now < scurve->segment[scurve->num_segs - 1 - i].end_time) {
            pnt = scurve->num_segs - 1 - i;
        }
    }
    if (pnt == 0) {
        Jtype = SCURVE_CONSTANT_JERK;
        Jm = 0.0f;
        tj = 0.0f;
        T0 = scurve->segment[pnt].end_time;
        A0 = scurve->segment[pnt].end_accel;
        V0 = scurve->segment[pnt].end_vel;
        P0 = scurve->segment[pnt].end_pos;
    } else if (pnt == scurve->num_segs) {
        Jtype = SCURVE_CONSTANT_JERK;
        Jm = 0.0f;
        tj = 0.0f;
        T0 = scurve->segment[pnt - 1].end_time;
        A0 = scurve->segment[pnt - 1].end_accel;
        V0 = scurve->segment[pnt - 1].end_vel;
        P0 = scurve->segment[pnt - 1].end_pos;
    } else {
        Jtype = scurve->segment[pnt].seg_type;
        Jm = scurve->segment[pnt].jerk_ref;
        tj = scurve->segment[pnt].end_time - scurve->segment[pnt - 1].end_time;
        T0 = scurve->segment[pnt - 1].end_time;
        A0 = scurve->segment[pnt - 1].end_accel;
        V0 = scurve->segment[pnt - 1].end_vel;
        P0 = scurve->segment[pnt - 1].end_pos;
    }

    switch (Jtype) {
    case SCURVE_CONSTANT_JERK:
        scurve_calc_javp_for_segment_const_jerk(time_now - T0, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
        break;
    case SCURVE_POSITIVE_JERK:
        scurve_calc_javp_for_segment_incr_jerk(time_now - T0, tj, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
        break;
    case SCURVE_NEGATIVE_JERK:
        scurve_calc_javp_for_segment_decr_jerk(time_now - T0, tj, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
        break;
    }
    *Pt_out = MAX(0.0f, *Pt_out);
}

// calculate the jerk, acceleration, velocity and position at time time_now when running the constant jerk time segment
static void scurve_calc_javp_for_segment_const_jerk(float time_now, float J0, float A0, float V0, float P0, float *Jt, float *At, float *Vt, float *Pt)
{
    *Jt = J0;
    *At = A0 + J0 * time_now;
    *Vt = V0 + A0 * time_now + 0.5f * J0 * (time_now * time_now);
    *Pt = P0 + V0 * time_now + 0.5f * A0 * (time_now * time_now) + (1.0f / 6.0f) * J0 * (time_now * time_now * time_now);
}

// Calculate the jerk, acceleration, velocity and position at time time_now when running the increasing jerk magnitude time segment based on a raised cosine profile
static void scurve_calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float *Jt, float *At, float *Vt, float *Pt)
{
    if (!math_flt_positive(tj)) {
        *Jt = 0.0;
        *At = A0;
        *Vt = V0;
        *Pt = P0;
        return;
    }
    const float Alpha = Jm * 0.5f;
    const float Beta = M_PI / tj;
    *Jt = Alpha * (1.0f - cosf(Beta * time_now));
    *At = A0 + Alpha * time_now - (Alpha / Beta) * sinf(Beta * time_now);
    *Vt = V0 + A0 * time_now + (Alpha * 0.5f) * (time_now * time_now) + (Alpha / (Beta * Beta)) * cosf(Beta * time_now) - Alpha / (Beta * Beta);
    *Pt = P0 + V0 * time_now + 0.5f * A0 * (time_now * time_now) + (-Alpha / (Beta * Beta)) * time_now + Alpha * (time_now * time_now * time_now) / 6.0f + (Alpha / (Beta * Beta * Beta)) * sinf(Beta * time_now);
}

// Calculate the jerk, acceleration, velocity and position at time time_now when running the decreasing jerk magnitude time segment based on a raised cosine profile
static void scurve_calc_javp_for_segment_decr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float *Jt, float *At, float *Vt, float *Pt)
{
    if (!math_flt_positive(tj)) {
        *Jt = 0.0;
        *At = A0;
        *Vt = V0;
        *Pt = P0;
        return;
    }
    const float Alpha = Jm * 0.5f;
    const float Beta = M_PI / tj;
    const float AT = Alpha * tj;
    const float VT = Alpha * ((tj * tj) * 0.5f - 2.0f / (Beta * Beta));
    const float PT = Alpha * ((-1.0f / (Beta * Beta)) * tj + (1.0f / 6.0f) * (tj * tj * tj));
    *Jt = Alpha * (1.0f - cosf(Beta * (time_now + tj)));
    *At = (A0 - AT) + Alpha * (time_now + tj) - (Alpha / Beta) * sinf(Beta * (time_now + tj));
    *Vt = (V0 - VT) + (A0 - AT) * time_now + 0.5f * Alpha * (time_now + tj) * (time_now + tj) + (Alpha / (Beta * Beta)) * cosf(Beta * (time_now + tj)) - Alpha / (Beta * Beta);
    *Pt = (P0 - PT) + (V0 - VT) * time_now + 0.5f * (A0 - AT) * (time_now * time_now) + (-Alpha / (Beta * Beta)) * (time_now + tj) + (Alpha / 6.0f) * (time_now + tj) * (time_now + tj) * (time_now + tj) + (Alpha / (Beta * Beta * Beta)) * sinf(Beta * (time_now + tj));
}

// increment time pointer and return the position, velocity and acceleration vectors relative to the origin
static void scurve_move_from_pos_vel_accel(scurve_t scurve, float dt, Vector3f_t *pos, Vector3f_t *vel, Vector3f_t *accel)
{
    scurve_advance_time(scurve, dt);
    float scurve_P1 = 0.0f;
    float scurve_V1, scurve_A1, scurve_J1;
    scurve_get_jerk_accel_vel_pos_at_time(scurve, scurve->time, &scurve_J1, &scurve_A1, &scurve_V1, &scurve_P1);

    pos->x += scurve->delta_unit.x * scurve_P1;
    pos->y += scurve->delta_unit.y * scurve_P1;
    pos->z += scurve->delta_unit.z * scurve_P1;

    vel->x += scurve->delta_unit.x * scurve_V1;
    vel->y += scurve->delta_unit.y * scurve_V1;
    vel->z += scurve->delta_unit.z * scurve_V1;

    accel->x += scurve->delta_unit.x * scurve_A1;
    accel->y += scurve->delta_unit.y * scurve_A1;
    accel->z += scurve->delta_unit.z * scurve_A1;

    scurve->position_sq = sq(scurve_P1);
}

// increment time pointer and return the position, velocity and acceleration vectors relative to the destination
static void scurve_move_to_pos_vel_accel(scurve_t scurve, float dt, Vector3f_t *pos, Vector3f_t *vel, Vector3f_t *accel)
{
    scurve_advance_time(scurve, dt);
    float scurve_P1 = 0.0f;
    float scurve_V1, scurve_A1, scurve_J1;
    scurve_get_jerk_accel_vel_pos_at_time(scurve, scurve->time, &scurve_J1, &scurve_A1, &scurve_V1, &scurve_P1);

    pos->x += scurve->delta_unit.x * scurve_P1;
    pos->y += scurve->delta_unit.y * scurve_P1;
    pos->z += scurve->delta_unit.z * scurve_P1;

    vel->x += scurve->delta_unit.x * scurve_V1;
    vel->y += scurve->delta_unit.y * scurve_V1;
    vel->z += scurve->delta_unit.z * scurve_V1;

    accel->x += scurve->delta_unit.x * scurve_A1;
    accel->y += scurve->delta_unit.y * scurve_A1;
    accel->z += scurve->delta_unit.z * scurve_A1;
 
    scurve->position_sq = sq(scurve_P1);

    pos->x -= scurve->track.x;
    pos->y -= scurve->track.y;
    pos->z -= scurve->track.z;
}

// return the position, velocity and acceleration vectors relative to the origin at a specified time along the path
static void scurve_move_from_time_pos_vel_accel(scurve_t scurve, float time_now, Vector3f_t *pos, Vector3f_t *vel, Vector3f_t *accel)
{
    float scurve_P1 = 0.0f;
    float scurve_V1 = 0.0f, scurve_A1 = 0.0f, scurve_J1 = 0.0f;
    scurve_get_jerk_accel_vel_pos_at_time(scurve, time_now, &scurve_J1, &scurve_A1, &scurve_V1, &scurve_P1);

    pos->x += scurve->delta_unit.x * scurve_P1;
    pos->y += scurve->delta_unit.y * scurve_P1;
    pos->z += scurve->delta_unit.z * scurve_P1;

    vel->x += scurve->delta_unit.x * scurve_V1;
    vel->y += scurve->delta_unit.y * scurve_V1;
    vel->z += scurve->delta_unit.z * scurve_V1;

    accel->x += scurve->delta_unit.x * scurve_A1;
    accel->y += scurve->delta_unit.y * scurve_A1;
    accel->z += scurve->delta_unit.z * scurve_A1;
}

// time at the end of the sequence
static inline float scurve_time_end(scurve_t scurve) 
{
    if (scurve->num_segs != segments_max) {
        return 0.0;
    }
    return scurve->segment[SCURVE_SEG_DECEL_END].end_time;
}

// time left before sequence will complete
static inline float scurve_get_time_remaining(scurve_t scurve)
{
    if (scurve->num_segs != segments_max) {
        return 0.0;
    }
    return scurve->segment[SCURVE_SEG_DECEL_END].end_time - scurve->time;
}

// time when acceleration section of the sequence will complete
static inline float scurve_get_accel_finished_time(scurve_t scurve)
{
    if (scurve->num_segs != segments_max) {
        return 0.0;
    }
    return scurve->segment[SCURVE_SEG_ACCEL_END].end_time;
}

// return true if the sequence is braking to a stop
static inline bool scurve_braking(scurve_t scurve)
{
    if (scurve->num_segs != segments_max) {
        return true;
    }
    return scurve->time >= scurve->segment[SCURVE_SEG_CONST].end_time;
}

// return time offset used to initiate the turn onto leg
static inline float scurve_time_turn_in(scurve_t scurve)
{
    if (scurve->num_segs != segments_max) {
        return 0.0;
    }
    return scurve->segment[SCURVE_SEG_TURN_IN].end_time;
}

// return time offset used to initiate the turn from leg
static inline float scurve_time_turn_out(scurve_t scurve)
{
    if (scurve->num_segs != segments_max) {
        return 0.0;
    }
    return scurve->segment[SCURVE_SEG_TURN_OUT].end_time;
}

// increment the internal time
static inline void scurve_advance_time(scurve_t scurve, float dt)
{
    scurve->time = MIN(scurve->time+dt, scurve_time_end(scurve));
}

// get desired maximum speed along track
static inline float scurve_get_speed_along_track(scurve_t scurve)
{
    return scurve->vel_max;
}

// get desired maximum acceleration along track
static inline float scurve_get_accel_along_track(scurve_t scurve)
{
    return scurve->accel_max;
}

// return the change in position from origin to destination
static inline const Vector3f_t scurve_get_track(scurve_t scurve)
{
    return scurve->track;
}

// return the current time elapsed
static inline float scurve_get_time_elapsed(scurve_t scurve)
{
    return scurve->time;
}
/*------------------------------------test------------------------------------*/


